#include <Wire.h>  
#define EEPROMAddress 0x50
//Basic pinouts ofr each motor
//Pin that is pulsed high/low to make motor 1 take a step
int D1Step = 47;
//This pin is either high (forward direction) or low (reverse)
int D1Dir = 46;
//Cuts or allows power tot he motor
int D1Enable = 53;
//Address in EEPROM to store motor data (not essential, but nice)
int D1address = 0;
int D1StepCounter = 0;

int D2Step = 45;
int D2Dir = 44;
int D2Enable = 52;
int D2address = 1;
int D2StepCounter = 0;

int D3Step = 43;
int D3Dir = 42;
int D3Enable = 51;
int D3address = 2;
int D3StepCounter = 0;

int D4Step = 41;
int D4Dir = 40;
int D4Enable = 50;
int D4address = 3;
int D4StepCounter = 0;

int D5Step = 39;
int D5Dir = 38;
int D5Enable = 49;
int D5address = 4;
int D5StepCounter = 0;

int D6Step = 37;
int D6Dir = 36;
int D6Enable = 48;
int D6address = 5;
int D6StepCounter = 0;

//Set of global string variables for storing/processing commands through the serial interface.
String command= "";
String token1= "";
String token2= "";
String token3= "";
//Arbitrary delay to give the drivers a little time to think as they are pulsed
float stepDelay = 1;
//This is the steps per MM based on the anaheim motors data sheet. (TODO: double check calculation)
double stepsPermm = 157;
boolean emergencyKill = false;


void setup() {
  Serial.begin(9600);
  Wire.begin();  
  initalizePins();
}

void initalizePins(){
  pinMode(23, OUTPUT);
  pinMode(25, OUTPUT);
  pinMode(27, OUTPUT);
  digitalWrite(23, HIGH);
  digitalWrite(25, LOW);
  digitalWrite(27, LOW);
  
  pinMode(D1Step, OUTPUT);
  pinMode(D1Dir, OUTPUT);
  pinMode(D1Enable, OUTPUT);
  digitalWrite(D1Step, LOW);
  digitalWrite(D1Dir, HIGH);
  digitalWrite(D1Enable, HIGH);

  pinMode(D2Step, OUTPUT);
  pinMode(D2Dir, OUTPUT);
  pinMode(D2Enable, OUTPUT);
  digitalWrite(D2Step, LOW);
  digitalWrite(D2Dir, HIGH);
  digitalWrite(D2Enable, HIGH);

  pinMode(D3Step, OUTPUT);
  pinMode(D3Dir, OUTPUT);
  pinMode(D3Enable, OUTPUT);
  digitalWrite(D3Step, LOW);
  digitalWrite(D3Dir, HIGH);
  digitalWrite(D3Enable, HIGH);

  pinMode(D4Step, OUTPUT);
  pinMode(D4Dir, OUTPUT);
  pinMode(D4Enable, OUTPUT);
  digitalWrite(D4Step, LOW);
  digitalWrite(D4Dir, HIGH);
  digitalWrite(D4Enable, HIGH);

  pinMode(D5Step, OUTPUT);
  pinMode(D5Dir, OUTPUT);
  pinMode(D5Enable, OUTPUT);
  digitalWrite(D5Step, LOW);
  digitalWrite(D5Dir, HIGH);
  digitalWrite(D5Enable, HIGH);

  pinMode(D6Step, OUTPUT);
  pinMode(D6Dir, OUTPUT);
  pinMode(D6Enable, OUTPUT);
  digitalWrite(D6Step, LOW);
  digitalWrite(D6Dir, HIGH);
  digitalWrite(D6Enable, HIGH);

  initAllSteps();
}

//Reads all positions (in MM) of each motors and prints them to the serial monitor
void readAllSteps(){
  D1StepCounter = (int)readEEPROM(D1address);
  delay(15);
  D2StepCounter = (int)readEEPROM(D2address);
  delay(15);
  D3StepCounter = (int)readEEPROM(D3address);
  delay(15);
  D4StepCounter = (int)readEEPROM(D4address);
  delay(15);
  D5StepCounter = (int)readEEPROM(D5address);
  delay(15);
  D6StepCounter = (int)readEEPROM(D6address);
  delay(15);

  Serial.print("Stepper 1 : ");
  Serial.print(D1StepCounter);
  Serial.println(" mm");

  Serial.print("Stepper 2 : ");
  Serial.print(D2StepCounter);
  Serial.println(" mm");

  Serial.print("Stepper 3 : ");
  Serial.print(D3StepCounter);
  Serial.println(" mm");

  Serial.print("Stepper 4 : ");
  Serial.print(D4StepCounter);
  Serial.println(" mm");

  Serial.print("Stepper 5 : ");
  Serial.print(D5StepCounter);
  Serial.println(" mm");

  Serial.print("Stepper 6 : ");
  Serial.print(D6StepCounter);
  Serial.println(" mm");
}

//Loads any saved positions on startup
void initAllSteps(){
  D1StepCounter = (int)readEEPROM(D1address);
  delay(15);
  D2StepCounter = (int)readEEPROM(D2address);
  delay(15);
  D3StepCounter = (int)readEEPROM(D3address);
  delay(15);
  D4StepCounter = (int)readEEPROM(D4address);
  delay(15);
  D5StepCounter = (int)readEEPROM(D5address);
  delay(15);
  D6StepCounter = (int)readEEPROM(D6address);
  delay(15);


}

//Write all positions to the EEPROM
void writeAllSteps(){
  writeEEPROM(D1address, D1StepCounter);
  delay(15);
  writeEEPROM(D2address, D2StepCounter);
  delay(15);
  writeEEPROM(D3address, D3StepCounter);
  delay(15);
  writeEEPROM(D4address, D4StepCounter);
  delay(15);
  writeEEPROM(D5address, D5StepCounter);
  delay(15);
  writeEEPROM(D6address, D6StepCounter);
  delay(15);
  Serial.println("All steps saved to EEPROM <Return>");
}
//Erases all saved steps from the EEPROM chip
void eraseAllSteps(){
  writeEEPROM(D1address, 0);
  D1StepCounter = 0;
  delay(15);
  writeEEPROM(D2address, 0);
  D2StepCounter = 0;
  delay(15);
  writeEEPROM(D3address, 0);
  D3StepCounter = 0;
  delay(15);
  writeEEPROM(D4address, 0);
  D4StepCounter = 0;
  delay(15);
  writeEEPROM(D5address, 0);
  D5StepCounter = 0;
  delay(15);
  writeEEPROM(D6address, 0);
  D6StepCounter = 0;
  delay(15);
  Serial.println("EEPROM erased <Return>");
}



void loop() {
  getCommand();
  chunk();
  interpret();
}


//Helper method for writing to EEPROM
void writeEEPROM(unsigned int eeaddress, byte data ) 
{
  Wire.beginTransmission(EEPROMAddress);
  Wire.write((int)(eeaddress >> 8));   // MSB
  Wire.write((int)(eeaddress & 0xFF)); // LSB
  Wire.write(data);
  Wire.endTransmission();

  delay(5);
}
//Helper method for reading from EEPROM
byte readEEPROM( unsigned int eeaddress ) 
{
  byte rdata = 0xFF;

  Wire.beginTransmission(EEPROMAddress);
  Wire.write((int)(eeaddress >> 8));   // MSB
  Wire.write((int)(eeaddress & 0xFF)); // LSB
  Wire.endTransmission();

  Wire.requestFrom(EEPROMAddress,1);

  if (Wire.available()) rdata = Wire.read();

  return rdata;
}



//wait until there is something in the serial buffer
//as soon as there is, clobber it, then chunk and interpret it.
void getCommand() {
  while (!Serial.available())
  {   
  }
  command="";
  while (Serial.available())
  {
    command+=char((Serial.read()));
    delay(2);
  }
  Serial.println(command);
  command.toLowerCase();
}
//split the command up by the ':' present in the string
void chunk() {
  int place1=command.indexOf(":");
  token1=command.substring(0,place1);
  int place2=command.indexOf(":",place1+1);
  token2=command.substring(place1+1,place2);
  token3=command.substring(place2+1, command.indexOf("<") - 1);
}
//Converts the code naming convention of the steppers to the user naming convention
String convertName(String name){
  if(name.indexOf("1") != -1){
    return "Stepper 1";
  }

  if(name.indexOf("2") != -1){
    return "Stepper 2";
  }

  if(name.indexOf("3") != -1){
    return "Stepper 3";
  }

  if(name.indexOf("4") != -1){
    return "Stepper 4";
  }

  if(name.indexOf("5") != -1){
    return "Stepper 5";
  }

  if(name.indexOf("6") != -1){
    return "Stepper 6";
  }
}


//Take the 3 token command and process it
void interpret() {
  if(token1 == "advance" || token1 == "a"){
    char buffer[50];
    token3.toCharArray(buffer,50);
    stepMotorUp(token2, atof(buffer));
    String message = "Advanced " + convertName(token2) + " by " + token3 + " mms " + "<Return>";

    Serial.println(message);
  }
  if(token1 == "retract" || token1 == "r"){
    char buffer[50];
    token3.toCharArray(buffer,50);
    stepMotorDown(token2, atof(buffer));
    String message = "Retracted " + convertName(token2) + " by " + token3 + " mms " + "<Return>";

    Serial.println(message);
  }

  if(token1 == "write" || token1 == "w"){
    if(token2 == "all" || token2 == "a"){
      if(token3 == "steps" || token3 == "s"){
        writeAllSteps();
      }
    }
  }

  if(token1 == "read" || token1 == "r"){
    if(token2 == "all" || token2 == "a"){
      if(token3 == "steps" || token3 == "s"){
        readAllSteps();
      }
    }
  }

  if(token1 == "erase" || token1 == "r"){
    if(token2 == "all" || token2 == "a"){
      if(token3 == "steps" || token3 == "s"){
        eraseAllSteps();
      }
    }
  }
}

void enableMotor(String motorName){
  if(motorName == "s1"){
    digitalWrite(D1Enable, LOW);
  }
  if(motorName == "s2"){
    digitalWrite(D2Enable, LOW);
  }
  if(motorName == "s3"){
    digitalWrite(D3Enable, LOW);
  }
  if(motorName == "s4"){
    digitalWrite(D4Enable, LOW);
  }
  if(motorName == "s5"){
    digitalWrite(D5Enable, LOW);
  }
  if(motorName == "s6"){
    digitalWrite(D6Enable, LOW);
  }
}

void disableMotor(String motorName){
  if(motorName == "s1"){
    digitalWrite(D1Enable, HIGH);
  }
  if(motorName == "s2"){
    digitalWrite(D2Enable, HIGH);
  }
  if(motorName == "s3"){
    digitalWrite(D3Enable, HIGH);
  }
  if(motorName == "s4"){
    digitalWrite(D4Enable, HIGH);
  }
  if(motorName == "s5"){
    digitalWrite(D5Enable, HIGH);
  }
  if(motorName == "s6"){
    digitalWrite(D6Enable, HIGH);
  }
}
//moves the selected motor in the "forwards" direction
//Converts mms into the number of steps needed then clocks the step pin of the driver that many times.
//speed can be modified by adjusting the stepDelay
void stepMotorUp(String motorName, float mms){
  enableMotor(motorName);
  int steps = mms * stepsPermm;
  int stepPin = getMotorStepPin(motorName);
  int dirPin = getMotorDirPin(motorName);
  digitalWrite(dirPin, HIGH);

  for( int i = 0; i < steps; i++){
    digitalWrite(stepPin, HIGH);
    delay(stepDelay);
    digitalWrite(stepPin, LOW);
    delay(stepDelay);
  }

  disableMotor(motorName);

}
//moves the selected motor in the "backwards" direction
void stepMotorDown(String motorName, float mms){
  enableMotor(motorName);
  int steps = mms * stepsPermm;
  int stepPin = getMotorStepPin(motorName);
  int dirPin = getMotorDirPin(motorName);
  digitalWrite(dirPin, LOW);

  for( int i = 0; i < steps; i++){
    digitalWrite(stepPin, HIGH);
    delay(stepDelay);
    digitalWrite(stepPin, LOW);
    delay(stepDelay);
  }

  disableMotor(motorName);

}
//Helper method to return the step pin of a motor based on its name

int getMotorStepPin(String name){
  if(name == "s1"){
    return 47; 
  }
  if(name == "s2"){
    return 45; 
  }
  if(name == "s3"){
    return 43; 
  }
  if(name == "s4"){
    return 41; 
  }
  if(name == "s5"){
    return 39; 
  }
  if(name == "s6"){
    return 37; 
  } 
}

//Helper method to return the direction pin of a motor based on its name
int getMotorDirPin(String name){
  if(name == "s1"){
    return 46; 
  }
  if(name == "s2"){
    return 44; 
  }
  if(name == "s3"){
    return 42; 
  }
  if(name == "s4"){
    return 40; 
  }
  if(name == "s5"){
    return 38; 
  }
  if(name == "s6"){
    return 36; 
  } 
}


























